
% 参考文献:https://blog.csdn.net/A1353731147/article/details/126713996
% https://blog.csdn.net/CHDAJ58/article/details/134782326
% 通过仿真可以看出来，如果需要开方32位的定点数，需要先扩成64的定点数才能保证精度不变
function sqrt_restoring()
    sqrt_result = my_sqrt(118*2^28*2^28);
    fprintf('sqrt_result = %d[%f]\n',sqrt_result, double(sqrt_result)/2^28);
end

% 输入1个Q30.32,输出平方根Q15.16
function sqrt_result = my_sqrt(q16_data)
    sqrt_result = uint64(0);
    r = uint64(0);
    for i = 31 : -1 : 0
        tracker = bitshift(sqrt_result, 2) + 1;
        % 余数左移2位
        remainder = bitshift(r, 2);
        % 提取位，按位合并
        remainder = bitor(remainder, extract_2bits(q16_data, 2*i + 1)); % 提取第 2*i+1 位和 2*i 位
        % 计算 q 和 r
        [q, r] = q_n(remainder, tracker);
        sqrt_result = bitshift(sqrt_result, 1);
        sqrt_result = bitor(sqrt_result,q);

%         binary_str = dec2bin(sqrt_result, 64);
%         fprintf('计算结果 :%02d:%s\n', 31-i,binary_str);

        remainder_str = dec2bin(remainder, 64);
        fprintf('余数:%02d:%s\n', 31-i,remainder_str);

%         tracker_str = dec2bin(tracker, 64);
%         fprintf('跟踪器:%02d:%s\n', 31-i,tracker_str);
    end
end

function [qn,rn] = q_n(qn_bit, a_mult_2_add1)
    if( qn_bit >= a_mult_2_add1 )
        qn = 1;
        rn = qn_bit - a_mult_2_add1;
    else
        qn = 0;
        rn = qn_bit;
    end
end
 % 提取a 的high_bit位和high_bit-1位
function extracted_bits = extract_2bits(a, high_bit)
    % 提取 a 的 high_bit 和 high_bit-1 位的值
    % a: 输入的 uint64 变量
    % high_bit: 要提取的最高位编号（如 7, 15, 19）
    % 检查输入的高位是否合法（需要在 [1, 31] 范围内）
    if high_bit > 63 || high_bit < 1
        error('high_bit 必须在 1 到 63 之间');
    end

    % 计算低位（低 1 位即为 high_bit - 1）
    low_bit = high_bit - 1;

    % 如果低位小于 0，表示输入不合法
    if low_bit < 0
        error('无法提取 2 位，high_bit 太小');
    end

    % 右移 low_bit 位，把要提取的两位移到最低有效位
    shifted_a = bitshift(a, -(low_bit));

    % 使用 bitand 掩码只保留最低 2 位
    extracted_bits = bitand(shifted_a, uint64(3));  % 掩码3 = '11' 二进制
end